#ifndef DECISION_H
#define DECISION_H

#include <string>
#include <vector>
#include "common/TrajectoryTracking_mag.h"
#include "common/pid_control.h"

#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "ubt_interface/msg/rfid.hpp"

#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>

#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "std_msgs/msg/u_int8.hpp"

// 二维码协议U1000-ii新
#include "ubt_interface/msg/nav_req.hpp"
#include  "ubt_interface/msg/nav_resp.hpp"
#include  "ubt_interface/msg/nav_info.hpp"

#include "ubt_interface/msg/nav_path_info.hpp"
#include "ubt_interface/msg/line.hpp"
#include "ubt_interface/msg/limit.hpp"
#include "ubt_interface/msg/point.hpp"

#include "std_msgs/msg/int32.hpp"
#include "ubt_interface/msg/pose2d_stamped.hpp"
#include "std_msgs/msg/u_int32.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#define MAGNETIC_SENSOR_UNMBER  	16     //磁条检测IO数量
#define MAGNETIC_SENSOR_INTERVAL  	0.010f     //磁条检测IO间隔
#include "common/node.h"

namespace control
{

enum VmsCmdExecuteState
{
	WAIT = 0,
	EXECUTING,
	FINISH,
	ABORT,
	ABORT_EXCEED_3M,
	ABORT_TRACK_FAILED,
	ABORT_POSE_JUMP
};

enum ExecutingStep
{
	TurnToNewLine = 0,
	TrackNewLine,
	TurnToStartPoint,
	TrackPath
};

enum SinglePathTrackStep
{
	Track = 0,
	Rotate
};

class Control_mag
{

public:

    Control_mag();// 构造函数
	~Control_mag() = default;		 // 析构函数

private:


	//需要订阅的消息
    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr location_sub_;	 // 订阅定位消息

    rclcpp::Subscription<geometry_msgs::msg::Twist >::SharedPtr agvstatus_sub_;	 // 订阅agvstatus消息
	rclcpp::Subscription<geometry_msgs::msg::Twist >::SharedPtr agvstatus_sub_1_;// 订阅agvstatus消息
	rclcpp::Subscription<std_msgs::msg::String >::SharedPtr ubt_state_sub_;  // 订阅ubt state状态机的状态值
	rclcpp::Subscription<std_msgs::msg::UInt8 >::SharedPtr stop_obs_sub_;
	rclcpp::Subscription<std_msgs::msg::String >::SharedPtr path_load_sub_;
	rclcpp::Subscription<std_msgs::msg::String >::SharedPtr init_find_rfid_sub_;
	rclcpp::Subscription<std_msgs::msg::Float32 >::SharedPtr angle_encoder_sub_;
	rclcpp::Subscription<std_msgs::msg::Bool >::SharedPtr standalone_mode_sub_; // 单机模式
	rclcpp::TimerBase::SharedPtr      timer_;

	//U1000-2协议添加
	rclcpp::Subscription<ubt_interface::msg::NavReq >::SharedPtr nav_cmd_sub_;// 订阅导航控制指令

	//需要发布的消息
	rclcpp::Publisher<std_msgs::msg::String>::SharedPtr init_find_rfid_pub;
    rclcpp::Publisher< geometry_msgs::msg::Twist >::SharedPtr control_cmd_pub_; // 发布命令（v,w）
	rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr path_info_pub_;   // 发布path point
	//rclcpp::Publisher< std_msgs::msg::Int32 >::SharedPtr path_index_pub_;  // 发布到达目标点的标志
	rclcpp::Publisher< std_msgs::msg::String >::SharedPtr arrivetarget_pub_;// 发布到达目标点的标志
	rclcpp::Publisher< ubt_interface::msg::NavResp>::SharedPtr arrivetarget_pub_qr;// 发布到达目标点的标志二维码版本
	rclcpp::Publisher< std_msgs::msg::Int32 >::SharedPtr path_index_mag_pub_;
	rclcpp::Subscription<std_msgs::msg::Int32 >::SharedPtr path_index_sub_;
	//输入信息处理函数
	void agvStateInfoCallback(const nav_msgs::msg::Odometry::SharedPtr agvstatus_msg);// agvstatus callback
	void uslamLocationCallback(const geometry_msgs::msg::Pose::SharedPtr location_msg);
	void locationCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr location_msg);
	void motorInfoCallback(const geometry_msgs::msg::Twist::SharedPtr agvstatus_msg);
	void ubtStateInfoCallback(const std_msgs::msg::String::SharedPtr ubt_state_msg);
	void stopObsCallback(const std_msgs::msg::UInt8::SharedPtr obstacle_msg);
	void pathLoadCallback(const std_msgs::msg::String::SharedPtr path_load_msg);
	void partPathInfoCallback(const ubt_interface::msg::NavPathInfo::SharedPtr nav_path_msg);
	void InitFindRfidCallBack(const std_msgs::msg::String::SharedPtr msg);
	void partPathInfoCallback_qr(const ubt_interface::msg::NavReq::SharedPtr nav_cmd_msg);
	void ChangePathIndexCallBack(const std_msgs::msg::Int32::SharedPtr msg);
	void standaloneModeCallback(const std_msgs::msg::Bool::SharedPtr msg);
	void SpdlogInit();
	void Init();			// 内部变量初始化
	void VarResetZero();    // 变量清零
	void TaskWait();        // 等待任务
	void TaskExecuting();   // 执行任务
	void TaskFinish();      // 完成任务
	void TurnToNewLine();   // 转向新的直线
	void TrackNewLine();    // 跟踪新的直线
	void TurnToStartPoint();// 转向起始点
	void TrackPath();       // 跟踪路径
	void JudgePoseJump();


	void task_management();	// 任务管理
	void GetPathInfo();		// 读取路径信息
	// 获取路径点的信息
	void GetPathPointInfo(std::vector<Pose_type> &pathPointInfo, const std::vector<ubt_interface::msg::Point> &pointInfo);
	void GetPathPointInfo_(std::vector<Pose_type> &pathPointInfo, const std::vector<Pose_type> &pointInfo);
	//void Location_QrCode_CallBack(const ubt_interface::msg::Pose2dStamped::SharedPtr  msg);
	void PublishSpeedOmgMsg(double v,double w);// 下发车辆的速度和旋转角速度信息（v,w）
	void PublishPathInfo(Pose_type pathinfo);
	void PublishArrivedTargetMsg(std::string arrived_flag);//发布到达目标点的标志
	void PublishPathIndexMsg(int path_index);
	void PublishRotateFinishMsg(int path_index);
	void PublishEstopMsg();	// 下发急停命令
	void PublishBrakeMsg(); // 滑停：下发减速停车命令
	void ContinuePauseRun();//继续功能
	void ContinueTrafficRun();//继续功能
	void TrafficRun(Pose_type traffic_pos);//交管路线规划
	void SlowDown();
	void updatePathInfo(void);
	//save data
	void CreatTraceFolder();
	void TraceData();
	void DeleteRedundantTraceFiles();
	void PublishSpeedMsg(double left,double right);

	std::string  path_type_;
	int  path_index_;
	int  new_path_index_;
	int  turn_direction_;
	bool read_yaml_flag_;
	bool navigation_start_;
	std::string  ubt_state_;
	std::string  ubt_state_pre_;
	std::string  ubt_state_two;
	std::string  path_load_;
	std::string  obs_stop_;

	////控制算法函数
	//设置PID参数函数
	void setOffsetyPID();
	void setHeadingPID();
	Track_mag track_mag_;

	TrajectoryTracking trajectorytracking_;
	TrajectoryTracking new_line_tracking_;
	int TraceDataCount;
	std::vector< std::string > control_data_vec_;

	uint8_t   stop_obs_flag_;
	geometry_msgs::msg::Pose2D center_point_,pre_center_point_;//车辆中心点位姿
	Pose_type centerpoint_init_;
	Pose_type projection_point_;

	double vehicle_speed_,vehicle_angular_;
	double vehicle_motor_speed_,vehicle_motor_angular_;
	double vehicle_odom_speed_,vehicle_odom_angular_,odom_pose_x_,odom_pose_y_,odom_pose_theta_;
	double cos_theta_;

	bool                CMD_finish_flag_;// 指令完成标志位
	std::string	        Task_complete_flag_;// 所有路径全部执行完成，到达最终目标点
	VmsCmdExecuteState  AgvExecuteCmdState_step_;// AGV指令执行状态（等待，执行中，执行完成，异常终止）
	ExecutingStep       ExecutingStep_;
	SinglePathTrackStep single_path_track_step_;

	PathInfoType              new_path_;
	PathInfoType              fack_path_;
	std::vector<PathInfoType> RouteInfo_,RouteInfo_save;
	std::vector<PathInfoType> New_route_;
	// 变量锁
    mutable boost::mutex location_mutex_;
	double  save_time_;
	double  track_time_;
	double  task_time_;
	clock_t task_start_;
	clock_t task_end_;
	clock_t save_start_;
	clock_t save_end_;
	clock_t track_start_;
	clock_t track_end_;

	//for test 20210715
	bool navigation_estop_flag_;
	double estop_point_x_;
	double estop_point_y_;
	double dis_tolerance_;

	// 模块准备信息
	bool Ready_agvstatus_;//agvstatus准备就绪
	bool Ready_location_; //位置信息准备就绪
	bool rotate_stop_flag_;
	bool Estop_flag_;	  //急停标志位
	bool Task_cancel_flag_;
	bool stop_save_data_flag_;
	int  stop_save_data_count_;
	int  state_value_;

	//控制命令
	PIDControl pid_control_offsety_;
	PIDControl pid_control_heading_;
	PIDConf pid_conf_offsety_;
	PIDConf pid_conf_heading_;
	double cycletime_;
	double vel_cmd_;
	double w_cmd_;
	double target_speed_;
	double init_angle_;
	double dec_new_;
	double travel_distance_;//add 20210315

	int stop_step_;
	int cycle_frequency_;
	int end_point_flag_;
	double position_jump_;
	double heading_jump_;
	double replan_angle_min_;
	double replan_angle_max_;
	double car_length_;
	double car_width_;
	double max_speed_;
	double max_speed_tolerance_;
	double max_turn_speed_;
	double max_speed_now_x;
	double max_speed_now_w;
	double two_line_length_;
	double track_new_line_speed_;
	double max_omg_;
	double current2start_dis_;
	double current2start_dis_max_;
	double actuator_delaytime_;
	double angle_down_;
	ControlParamType controlParamInfo_;


	void TrackMagPath();       // 跟踪路径
	void Code_Mag_CallBack(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
	void MaxSpeddCallBack(const std_msgs::msg::Float32::SharedPtr msg);
	void getParam();       // 跟踪路径
	void RfidCallBack(const ubt_interface::msg::Rfid::SharedPtr msg);
	void LiftCallBack(const std_msgs::msg::UInt32::SharedPtr msg);
	void AngleEncoderCallback(const std_msgs::msg::Float32::SharedPtr angle);
	void TurnFoot(int way);
	void FindMag();
	void TimeFindRfid();
	void CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr  speed);
	void DirectCmdVelCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg);
	void CharacteristicCallBack(const std_msgs::msg::UInt8::SharedPtr msg);

	rclcpp::Subscription<std_msgs::msg::UInt8 >::SharedPtr characteristic_sub_;
	rclcpp::Subscription<std_msgs::msg::Int32MultiArray >::SharedPtr code_mag_sub_;	 // 订阅定位消息
	rclcpp::Subscription<std_msgs::msg::Float32 >::SharedPtr max_speed_sub_;
	rclcpp::Subscription<ubt_interface::msg::Rfid >::SharedPtr rfid_sub_;
	rclcpp::Subscription<std_msgs::msg::UInt32 >::SharedPtr lift_sub_;
	rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr mdirect_cmd_vel_sub_;
	rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

	rclcpp::TimerBase::SharedPtr      get_param_timer_;
	rclcpp::TimerBase::SharedPtr      findRfid_timer_;
	rclcpp::Subscription<ubt_interface::msg::NavReq >::SharedPtr nav_req_mag_sub_;
	rclcpp::Publisher< std_msgs::msg::Float32MultiArray >::SharedPtr control_mage_cmd_pub_; // 发布命令mag （left,right）
	rclcpp::Publisher< std_msgs::msg::Float32MultiArray >::SharedPtr control_mage_debug_pub_;
	rclcpp::Publisher< std_msgs::msg::Int32>::SharedPtr control_type_pub_;
	uint8_t target_mag_ = 1;
	double offset_mag_ = 0;
	int on_mag_ = 0;
	double  left_speed_;
    double  right_speed_;
	double receive_mage_time_ = 0;
	bool find_rfid_=false;
	int find_rfid_step_=0;
	int timer_start_=0;
	int find_rfid_flag_=0;
	Pose_type save_piont;
	double real_send_speed_;
	uint32_t   characteristic_scan_flag_ = 0;
	std::string oldRfid;
	uint32_t  last_rfid_is_induction_  = 0;
	bool  find_last_rfid_  = false;
	bool standalone_mode_ = false;
};

}
#endif

